#include <Arduino.h>
#include "CatRobot.h"
#include <SoftwareSerial.h>

// 构造函数
CatRobot::CatRobot() {
  // 初始化参数（与原程序一致）
  for (int i = 0; i < SERVONUM; i++) {
    servodeg0[i] = 90;
    servodegnew[i] = 90;
    servodegold[i] = 90;
    jointangle0[i] = (i % 3 == 0) ? alfa0 : ((i % 3 == 1) ? beta0 : 0);
  }
  int temp[] = {+1, +1, -1, -1, -1, +1, -1, -1, -1, +1, +1, +1};
  for (int i = 0; i < SERVONUM; i++) {
    servodir[i] = temp[i];
  }
  
  
  timestep = 500;
  steplength = 40;
  phi = 20;
}

// 初始化伺服引脚
void CatRobot::initServos() {
  servo[0].attach(4);
  servo[1].attach(5);
  servo[2].attach(3);
  servo[3].attach(10);
  servo[4].attach(11);
  servo[5].attach(9);
  servo[6].attach(7);
  servo[7].attach(8);
  servo[8].attach(6);
  servo[9].attach(13);
  servo[10].attach(A0);
  servo[11].attach(12);
  
  for (int i = 0; i < SERVONUM; i++) {
    servo[i].write(servodeg0[i]);
    servodegold[i] = servodeg0[i];
  }
}

void CatRobot::lengthangles() {
  // Front left foot
  jointangle[2] = gammaleft(footpos[1], footpos[2]);
  leglength[0] = legleft(footpos[0], footpos[2], jointangle[2]);
  jointangle[1] = beta(leglength[0]);
  jointangle[0] = alfafront(footpos[0], jointangle[1], leglength[0]);
  // Rear left foot
  jointangle[5] = gammaleft(footpos[4], footpos[5]);
  leglength[1] = legleft(footpos[3], footpos[5], jointangle[5]);
  jointangle[4] = beta(leglength[1]);
  jointangle[3] = alfarear(footpos[3], jointangle[4], leglength[1]);
  // Front rigth foot
  jointangle[8] = gammaright(footpos[7], footpos[8]);
  leglength[2] = legright(footpos[6], footpos[8], jointangle[8]);
  jointangle[7] = beta(leglength[2]);
  jointangle[6] = alfafront(footpos[6], jointangle[7], leglength[2]);
  // Rear right foot
  jointangle[11] = gammaright(footpos[10], footpos[11]);
  leglength[3] = legright(footpos[9], footpos[11], jointangle[11]);
  jointangle[10] = beta(leglength[3]);
  jointangle[9] = alfarear(footpos[9], jointangle[10], leglength[3]);
}

float CatRobot::gammaleft(float dy, float dz) {
  float gresult = atan((toeout0 + dy) / (height0 - dz)) - gamma0;
  return gresult;
}

float CatRobot::gammaright(float dy, float dz) {
  float gresult = gamma0 - atan((toeout0 - dy) / (height0 - dz));
  return gresult;
}

float CatRobot::legleft(float dx, float dz, float gamma) {
  float lresult = sqrt(pow(leglength0 - (dz / cos(gamma0 + gamma)), 2) + pow(dx, 2));
  if (lresult > 2 * jointlength) lresult = 2 * jointlength; // If leglength is higher than possible some following functions become unstable
  return lresult;
}

float CatRobot::legright(float dx, float dz, float gamma) {
  float lresult = sqrt(pow(leglength0 - (dz / cos(gamma0 - gamma)), 2) + pow(dx, 2));
  if (lresult > 2 * jointlength) lresult = 2 * jointlength; // If leglength is higher than possible some following functions become unstable
  return lresult;
}

float CatRobot::beta(float leg) {
  float bresult = 2 * acos(leg / (2 * jointlength));
  return bresult;
}

float CatRobot::alfafront(float dx, float beta, float leg) {
  float aresult = (beta / 2) - asin(dx / leg);
  return aresult;
}

float CatRobot::alfarear(float dx, float beta, float leg) {
  float aresult = (beta / 2) + asin(dx / leg);
  return aresult;
}

void  CatRobot::turnpos(float f) {
  stepturn[0] = bodyradius * cos(phi0 + (f * pi / 180)) - leng / 2;
  stepturn[1] = bodyradius * sin(phi0 + (f * pi / 180)) - width / 2;
  stepturn[3] = bodyradius * cos(pi - phi0 + (f * pi / 180)) + leng / 2;
  stepturn[4] = bodyradius * sin(pi - phi0 + (f * pi / 180)) - width / 2;
  stepturn[6] = bodyradius * cos(2 * pi - phi0 + (f * pi / 180)) - leng / 2;
  stepturn[7] = bodyradius * sin(2 * pi - phi0 + (f * pi / 180)) + width / 2;
  stepturn[9] = bodyradius * cos(pi + phi0 + (f * pi / 180)) + leng / 2;
  stepturn[10] = bodyradius * sin(pi + phi0 + (f * pi / 180)) + width / 2;
}

void CatRobot::servopos() {
  for (int i = 0; i < 12; i++) {
    servodegnew[i] = servodeg0[i] + servodir[i] * (jointangle[i] - jointangle0[i]) * 180 / pi;
  }
}

void CatRobot::servomove() {
  int servotimeold[SERVONUM]; // Local variable for time of last servo position
  int servotimenew[SERVONUM]; // Local variable for the current time when the servo i positioned
  int SERVOPULSE[SERVONUM]; // Local variable to write to the servo driver
  float servodeg[SERVONUM]; // Local variable for the current servo position
  float servodegspeed[SERVONUM]; // Local variable for the desired servo speed degress per millisecond
  unsigned long starttime = millis(); // Time stamp the start of the algorithm
  unsigned long timenow = starttime; // Resetting time now
  for (int i = 0; i < SERVONUM; i++) {
    servodegspeed[i] = (servodegnew[i] - servodegold[i]) / timestep; // Calculate the desired servo speed
    servodeg[i] = servodegold[i]; // Resetting the servo position
    servotimeold[i] = starttime; // Resetting the time
  }
  while ((timenow - starttime) < timestep) { // Loop continues until the time step is fulfilled
    for (int i = 0; i < SERVONUM; i++) { // Iterate through each servo
      servotimenew[i] = millis(); // Get a time stamp
      servodeg[i] += servodegspeed[i] * (servotimenew[i] - servotimeold[i]);
      // Calculate a new position based on the desired speed and elapsed time
      servo[i].write(servodeg[i]); // Position servo
      servotimeold[i] = servotimenew[i]; // Resetting the old servo time for the next iteration
    }
    timenow = millis();
    // Get a time stamp after all servos has been iterated to use in the while case.
  }
  for (int i = 0; i < SERVONUM; i++) { // Make on last iteration to assure that the servos reached their end positions
    servo[i].write(servodegnew[i]); // Position servo
    servodegold[i] = servodegnew[i]; // Resetting the current position for future iterations
  }
}

void CatRobot::servomovefast() {
  for (int i = 0; i < SERVONUM; i++) { // Make on last iteration to assure that the servos reached their end positions
    servo[i].write(servodegnew[i]); // Position servo
    servodegold[i] = servodegnew[i]; // Resetting the current position for future iterations
  }
  delay(100); // Just give a reasonable time for servos to reach endpoint before moving on.
}

void CatRobot::footxyz(int i, float x, float y, float z) {
  footpos[3 * i] = x;
  footpos[3 * i + 1] = y;
  footpos[3 * i + 2] = z;
  lengthangles();
  servopos();
}

void CatRobot::footmovexyz(int i, float x, float y, float z) {
  footpos[3 * i] += x;
  footpos[3 * i + 1] += y;
  footpos[3 * i + 2] += z;
  lengthangles();
  servopos();
}

void CatRobot::bodyxyz(float x, float y, float z ) {
  //Note: Moving body means moving the feet in the other direction, hence minus signs in all foot positions
  for (int i = 0; i < 4; i++) {
    footpos[3 * i] = -x;
    footpos[3 * i + 1] = -y;
    footpos[3 * i + 2] = -z;
  }
  lengthangles();
  servopos();
}

void CatRobot::bodymovexyz(float x, float y, float z ) {
  //Note: Body move mean moving the feet in the other direction, hence minus signs in all foot positions
  for (int i = 0; i < 4; i++) {
    footpos[3 * i] -= x;
    footpos[3 * i + 1] -= y;
    footpos[3 * i + 2] -= z;
  }
  lengthangles();
  servopos();
}

void CatRobot::bodytwist(float f) {
  // Again here the movement is in minus driection from the foot positions
  turnpos(-f);
  for (int i = 0; i < 12; i++) {
    footpos[i] += stepturn[i];
  }
  lengthangles();
  servopos();
}

void CatRobot::footstep (int i, float x, float y) {
  footmovexyz(i, 0, 0, 30);
  servomovefast();
  footmovexyz(i, x, y, 0);
  servomovefast();
  footmovexyz(i, 0, 0, -30);
  servomovefast();
}

void CatRobot::footstepangle(int i, float f) {
  turnpos(f);
  footmovexyz(i, 0, 0, 30);
  servomovefast();
  footmovexyz(i, stepturn[3 * i], stepturn [3 * i + 1], 0);
  servomovefast();
  footmovexyz(i, 0, 0, -30);
  servomovefast();
}

void CatRobot::forwardcreep() {
  bodymovexyz(steplength / 4, -toeout0, 0); // 开始向前行走的位置，向右倾斜
  servomove();
  footstep(1, steplength / 2, 0); // 移动左后腿半步长
  footstep(0, steplength / 2, 0); // 前面的左边
  bodymovexyz(steplength / 4, 2 * toeout0, 0); // 向前和向左移动身体（以便稍后移动右脚）
  servomove();
 {
    // 在此开始 while 循环，只要 fwd 被命令，就重复 (模式 1)
    footstep(3, steplength, 0); // Moving rear right forward
    footstep(2, steplength, 0); // Moving front right forward
    bodymovexyz(steplength / 2, -2 * toeout0, 0); // Shifting body forward and to the right
    servomove();
    footstep(1, steplength, 0); // Moving rear left forward
    footstep(0, steplength, 0); // Moving front left forward
    bodymovexyz(steplength / 2, 2 * toeout0, 0); // Shifting body forward and to the left
    servomove();
 }
    // The robot has the same position as before the while loop but has moved on steplength forward.
  // The while loop ends and it assumes normal postion
  /* bodymovexyz(0, 10, 0);*/
  footstep(3, steplength / 2, 0); // Taking half steps to make all legs neutral
  footstep(2, steplength / 2, 0);
  bodyxyz(0, 0, 0); // Centering body
  servomove();
  // Leaving gait mode
}

void CatRobot::reversecreep() {
  bodymovexyz(-steplength / 4, -toeout0, 0); // Starts to position for forward walking
  servomove();
  footstep(0, -steplength / 2, 0);
  footstep(1, -steplength / 2, 0);
  bodymovexyz(-steplength / 4, 2 * toeout0, 0);
  servomove();
{
    // Here the while loop starts, repeaetd as long as reverse is ordered (mode 2)
    footstep(2, -steplength, 0);
    footstep(3, -steplength, 0);
    bodymovexyz(-steplength / 2, -2 * toeout0, 0);
    servomove();
    footstep(0, -steplength, 0);
    footstep(1, -steplength, 0);
    bodymovexyz(-steplength / 2, 2 * toeout0, 0);
    servomove();
  }
  // The while loop ends and it assumes normal postion
  /*  bodymovexyz(0, 10, 0);*/
  footstep(2, -steplength / 2, 0);
  footstep(3, -steplength / 2, 0);
  bodyxyz(0, 0, 0);
  servomove();
  // Leaving gait mode
}

void CatRobot::leftturn() {
{
    // While loop as long as the left button is pressed
    bodyxyz(toeout0 / 2, toeout0, 0); // Lean left before doing anything
    servomove();
    footstepangle(3, phi); // Move rear right foot into new position
    footstepangle(2, phi); // Move front right foot into new position
    footxyz(0, -toeout0 / 2 - stepturn[0], toeout0 - stepturn[1], 0);
    footxyz(1, -toeout0 / 2 - stepturn[3], toeout0 - stepturn[4], 0);
    footxyz(2, -toeout0 / 2, toeout0, 0);
    footxyz(3, -toeout0 / 2, toeout0, 0);
    // Twisting body and lean left. Written in absolute coordinates to minmize errors.
    servomove(); // Do the actual servo command
    footstepangle(0, phi); // Move front left foot
    footstepangle(1, phi); // Move rear left foot
  }
  bodyxyz(0, 0, 0); // Centre body when turning is finished
  servomove();
}

void CatRobot::rightturn() {
{
    // While loop as long as the right button is pressed
    bodyxyz(-toeout0 / 2, toeout0, 0); // Lean left before doing anything
    servomove();
    footstepangle(2, -phi); //Move front right foot
    footstepangle(3, -phi); //Move rear right foot
    footxyz(0, toeout0 / 2 - stepturn[0], toeout0 - stepturn[1], 0);
    footxyz(1, toeout0 / 2 - stepturn[3], toeout0 - stepturn[4], 0);
    footxyz(2, toeout0 / 2, toeout0, 0);
    footxyz(3, toeout0 / 2, toeout0, 0);
    // Twisting body and lean left. Written in absolute coordinates to minmize errors.
    servomove(); // Do the actual servo command
    footstepangle(1, -phi); //Move rear left foot
    footstepangle(0, -phi); //Move front left foot
  }
  bodyxyz(0, 0, 0);
  servomove();
}

void CatRobot::sit(){
  for(int i=90; i>=40; i--)
  {
    servo[0].write(i);
    servo[6].write(180-i);
    servo[3].write(i);
    servo[9].write(180-i);
    delay(15);
    }

  for(int i=90; i>=35; i--)
  {
    servo[4].write(i);
    servo[10].write(180-i);
    delay(20);
    }
    
  for(int i=40, j=90; i<=90, j>=40; i++, j--)
  {
    servo[0].write(i);
    servo[6].write(180-i);
    servo[1].write(j);
    servo[7].write(180-j);
    delay(20);
    }
/*  servo[0].write(90);
  servo[1].write(40);
  servo[2].write(90);
  servo[3].write(40);
  servo[4].write(35);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(140);
  servo[8].write(90);
  servo[9].write(140);
  servo[10].write(145);
  servo[11].write(90);*/

  }

void CatRobot::stand(){
  /*servo[0].write(90);
  servo[1].write(40);
  servo[2].write(90);
  servo[3].write(40);
  servo[4].write(35);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(140);
  servo[8].write(90);
  servo[9].write(140);
  servo[10].write(145);
  servo[11].write(90);*/

  delay(1000);
  
  for(float i=40, j=35; i<=90, j<=90; i+=0.909, j+=1)
  {
    servo[1].write(i);
    servo[3].write(i);
    servo[4].write(j);
    servo[7].write(180-i);
    servo[9].write(180-i);
    servo[10].write(180-j);
    delay(20);
    }
  
  }

void CatRobot::handShake(){
  sit();
  delay(1000);

  servo[0].write(90);
  servo[1].write(40);
  servo[2].write(90);
  servo[3].write(40);
  servo[4].write(35);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(140);
  servo[8].write(90);
  servo[9].write(140);
  servo[10].write(145);
  servo[11].write(90);

  for(int i=90; i>=80; i--)
  {
    servo[2].write(i);
    servo[8].write(i);
    delay(60);
  }
  
  delay(500);
  
  for(int i=90; i<=110; i++)
  { 
    servo[2].write(180-i);
    servo[0].write(i);
    delay(15);
  }
  
  for(int i=100; i>=10; i--)
  {
    servo[0].write(i);
    delay(20);
  }
  
  for(int i=40; i<=60; i++)
  {
    servo[1].write(i);
    delay(20);
  }

  delay(5000);

 for(float i=10, j=60, k=70, l=80; i<=90, j>=40, k<=90, l<=90; i++, j-=0.25, k+=0.25, l+=0.125)
  {
    servo[0].write(i);
    servo[1].write(j);
    servo[2].write(k);
    servo[8].write(l);
    delay(20);
  }

  /*for(int i=10; i<=90; i++)
  {
    servo[0].write(i);
    delay(15);
  }

  for(int j=60; j>=40; j--)
  {
    servo[1].write(j);
    delay(12);
  }
  
  for(int i=70, j=80; i<=80, j<=90; i++, j++)
  { 
    servo[2].write(i);
    servo[8].write(j);
    delay(20);
  }
  
  for(int i=80; i<=90; i++)
  { 
    servo[2].write(i);
    delay(20);
  }*/
  
  delay(1000);// EXTRA
  
  stand();
  
/*  servo[0].write(90);
  servo[1].write(40);
  servo[2].write(90);
  servo[3].write(40);
  servo[4].write(35);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(140);
  servo[8].write(90);
  servo[9].write(140);
  servo[10].write(145);
  servo[11].write(90);
  */
}

void CatRobot::dance1(){
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  servo[4].write(90);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(90);
  servo[8].write(90);
  servo[9].write(90);
  servo[10].write(90);
  servo[11].write(90);
  
  for(int i=90; i<=110; i++)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(i);
    servo[7].write(i);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(i);
    servo[8].write(i);
    delay(20);
    }

  for(int i=110; i>=70; i--)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(i);
    servo[7].write(i);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(i);
    servo[8].write(i);
    delay(20);
    }

  for(int i=70; i<=110; i++)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(i);
    servo[7].write(i);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(i);
    servo[8].write(i);
    delay(20);
    }
    
  for(int i=110; i>=70; i--)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(i);
    servo[7].write(i);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(i);
    servo[8].write(i);
    delay(20);
    }

  for(int i=70; i<=110; i++)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(i);
    servo[7].write(i);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(i);
    servo[8].write(i);
    delay(20);
    }
  
  for(int i=110; i>=90; i--)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(i);
    servo[7].write(i);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(i);
    servo[8].write(i);
    delay(20);
    }
    
  delay(2000);
  
  }

void CatRobot::dance2(){
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  servo[4].write(90);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(90);
  servo[8].write(90);
  servo[9].write(90);
  servo[10].write(90);
  servo[11].write(90);
  
  for(int i=90, j=90; i<=110, j>=70; i++, j--)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(j);
    servo[7].write(j);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(j);
    servo[8].write(j);
    delay(20);
    }

  for(int i=110, j=70; i>=70, j<=110; i--, j++)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(j);
    servo[7].write(j);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(j);
    servo[8].write(j);
    delay(20);
    }

  for(int i=70, j=110; i<=110, j>=70; i++, j--)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(j);
    servo[7].write(j);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(j);
    servo[8].write(j);
    delay(20);
    }
    
  for(int i=110, j=70; i>=70, j<=110; i--, j++)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(j);
    servo[7].write(j);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(j);
    servo[8].write(j);
    delay(20);
    }

  for(int i=70, j=110; i<=110, j>=70; i++, j--)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(j);
    servo[7].write(j);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(j);
    servo[8].write(j);
    delay(20);
    }
  
  for(int i=110, j=70; i>=90, j<=90; i--, j++)
  {
    servo[4].write(i);
    servo[10].write(i);
    servo[1].write(j);
    servo[7].write(j);
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(j);
    servo[8].write(j);
    delay(20);
    }
    
  delay(2000);
}
  
void CatRobot::dance3(){
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  servo[4].write(90);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(90);
  servo[8].write(90);
  servo[9].write(90);
  servo[10].write(90);
  servo[11].write(90);

  for(int i=90; i>=40; i--)
  {
    servo[0].write(i);
    servo[6].write(180-i);
    servo[3].write(i);
    servo[9].write(180-i);
    delay(50);
    }

  for(int i=40; i>=10; i--)
  {
    servo[0].write(i);
    servo[6].write(180-i);
    servo[1].write(i+50);
    servo[7].write((180-i)-50);
    delay(50);
    }

  for(int i=40, j=90; i<=70, j<=120; i++, j++)
  {
    servo[3].write(i);
    servo[9].write(180-i);
    servo[4].write(j);
    servo[10].write(180-j);
    delay(20);
    }

  for(int i=90; i<=110; i++)
  {
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(180-i);
    servo[8].write(180-i);
    delay(20);
    }
    
  for(int i=110; i>=70; i--)
  {
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(180-i);
    servo[8].write(180-i);
    delay(20);
    }    

   for(int i=70; i<=110; i++)
  {
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(180-i);
    servo[8].write(180-i);
    delay(20);
    }
    
  for(int i=110; i>=70; i--)
  {
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(180-i);
    servo[8].write(180-i);
    delay(20);
    }    

  for(int i=70; i<=90; i++)
  {
    servo[5].write(i);
    servo[11].write(i);
    servo[2].write(180-i);
    servo[8].write(180-i);
    delay(20);
    }
delay(2000);
  for(float i=10, j=60, k=70, l=120; i<=90, j<=90, k<=90, l>=90; i+=3.33, j+=1.25, k+=0.83, l-=1.25)
  {
    servo[0].write(i);
    servo[1].write(j);
    servo[3].write(k);
    servo[4].write(l);
    servo[6].write(180-i);
    servo[7].write(180-j);
    servo[9].write(180-k);
    servo[10].write(180-l);
    delay(20);
    }
}

void CatRobot::kick(){
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  servo[4].write(90);
  servo[5].write(90);
  servo[6].write(90);
  servo[7].write(90);
  servo[8].write(90);
  servo[9].write(90);
  servo[10].write(90);
  servo[11].write(90);
  
  for(int i=90, j=90; i<=110, j>=70; i++, j--)
  {
    servo[2].write(j);
    servo[8].write(j);
    servo[1].write(j);
    servo[7].write(j);
    servo[4].write(i);
    servo[5].write(i);
    servo[11].write(i);
    servo[10].write(i);
    delay(50);
    }
    
delay(1000);
    
  for(int i=90, j=70;  i<=110, j<=110; i++, j+=2)
  {
    servo[0].write(i);
    servo[1].write(j);   
    delay(50);
    }
    
delay(500);

  for(int i=110, j=110;  i>=10, j>=60; i-=2, j--)
  {
    servo[0].write(i);
    servo[1].write(j);   
    delay(5);
    }

  for(float i=10, j=60, k=70, l=110;  i<=90, j<=90, k<=90, l>=90; i+=3.33, j+=1.25, k+=0.83, l-=0.83)
  {  
    servo[0].write(i);
    servo[1].write(j);
    servo[2].write(k);
    servo[4].write(l);
    servo[5].write(l);
    servo[7].write(k);
    servo[8].write(k);
    servo[10].write(l);
    servo[11].write(l);
    delay(50);
    }
  
  }


